/**
 * Copyright 2017 Decawave Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*! ----------------------------------------------------------------------------
 *  @file    main.c
 *  @brief   main loop for the DecaRanging application
 *
 */

/* Includes */
#include "dw1000_port.h"
#include "dw1000_instance.h"
#include "instance_main.h"
#include "dw1000_device_api.h"
#include "cmsis_os.h"
#include <stdio.h>
#include "dw1000_types.h"

instance_data_t instance_data[NUM_INST] ;

#define SWS1_SHF_MODE 0x02  //short frame mode (6.81M)
#define SWS1_CH5_MODE 0x04  //channel 5 mode
#define SWS1_ANC_MODE 0x08  //anchor mode
#define SWS1_A1A_MODE 0x10  //anchor/tag address A1
#define SWS1_A2A_MODE 0x20  //anchor/tag address A2
#define SWS1_A3A_MODE 0x40  //anchor/tag address A3
#define SWS1_USB2SPI_MODE 0x78  //USB to SPI mode
#define SWS1_TXSPECT_MODE 0x38  //Continuous TX spectrum mode
                             //"1234567812345678"
#define SOFTWARE_VER_STRING    "Ver.  2.10  TREK" //16 bytes!

uint8 s1switch = 0;
int instance_anchaddr = 0;
int dr_mode = 0;
int chan, tagaddr, ancaddr;
int instance_mode = ANCHOR;

typedef struct
{
    uint8 channel ;
    uint8 prf ;
    uint8 datarate ;
    uint8 preambleCode ;
    uint8 preambleLength ;
    uint8 pacSize ;
    uint8 nsSFD ;
    uint16 sfdTO ;
} chConfig_t ;


//Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )
chConfig_t chConfig[4] ={
                    //mode 1 - S1: 2 off, 3 off
                    {
                        2,              // channel
                        DWT_PRF_16M,    // prf
                        DWT_BR_110K,    // datarate
                        4,              // preambleCode
                        DWT_PLEN_1024,  // preambleLength
                        DWT_PAC32,      // pacSize
                        1,       // non-standard SFD
                        (1025 + 64 - 32) //SFD timeout
                    },
                    //mode 2 - S1: 2 on, 3 off
                    {
                        2,              // channel
                        DWT_PRF_16M,    // prf
                        DWT_BR_6M8,    // datarate
                        4,             // preambleCode
                        DWT_PLEN_128,   // preambleLength
                        DWT_PAC8,       // pacSize
                        0,       // non-standard SFD
                        (129 + 8 - 8) //SFD timeout
                    },
                    //mode 3 - S1: 2 off, 3 on
                    {
                        5,              // channel
                        DWT_PRF_16M,    // prf
                        DWT_BR_110K,    // datarate
                        3,              // preambleCode
                        DWT_PLEN_1024,  // preambleLength
                        DWT_PAC32,      // pacSize
                        1,       // non-standard SFD
                        (1025 + 64 - 32) //SFD timeout
                    },
                    //mode 4 - S1: 2 on, 3 on
                    {
                        5,              // channel
                        DWT_PRF_16M,    // prf
                        DWT_BR_6M8,    // datarate
                        3,             // preambleCode
                        DWT_PLEN_128,   // preambleLength
                        DWT_PAC8,       // pacSize
                        0,       // non-standard SFD
                        (129 + 8 - 8) //SFD timeout
                    }
};

//Slot and Superframe Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )
sfConfig_t sfConfig[4] ={
                    //mode 1 - S1: 2 off, 3 off
                    {
                        (28), //ms -
                        (10),   //thus 10 slots - thus 280ms superframe means 3.57 Hz location rate (10 slots are needed as AtoA ranging takes 30+ ms)
                        (10*28), //superframe period
                        (10*28), //poll sleep delay
                        (20000)
                    },
                    //mode 2 - S1: 2 on, 3 off
                    {
                        (10),   // slot period ms
                        (10),   // number of slots (only 10 are used) - thus 100 ms superframe means 10 Hz location rate
                        (10*10), // superframe period (100 ms - gives 10 Hz)
                        (10*10), // poll sleep delay (tag sleep time, usually = superframe period)
#ifdef NORDIC_NRF52840_BOARD
                        (4500)
#else
                        (2500)
#endif
                    },
                    //mode 3 - S1: 2 off, 3 on
                    {
                        (28),    // slot period ms
                        (10),     // thus 10 slots - thus 280ms superframe means 3.57 Hz location rate
                        (10*28),  // superframe period
                        (10*28),  // poll sleep delay
                        (20000)
                    },
                    //mode 4 - S1: 2 on, 3 on
                    {
                        (10),   // slot period ms
                        (10),   // number of slots (only 10 are used) - thus 100 ms superframe means 10 Hz location rate
                        (10*10), // superframe period (100 ms - gives 10 Hz)
                        (10*10), // poll sleep (tag sleep time, usually = superframe period)
#ifdef NORDIC_NRF52840_BOARD
                        (4500)
#else
                        (2500) // this is the Poll to Final delay - 2ms (NOTE: if using 6.81 so only 1 frame per ms allowed LDC)
#endif
                    }
};
// ======================================================
//
//  Configure instance tag/anchor/etc... addresses
//
void addressconfigure(uint8 s1switch, uint8 mode)
{
    uint16 instAddress ;

    instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4;

    if(mode == ANCHOR)
    {
        if(instance_anchaddr > 3)
        {
            instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener
        }
        else
        {
            instAddress = GATEWAY_ANCHOR_ADDR | instance_anchaddr;
        }
    }
    else
    {
        instAddress = instance_anchaddr;
    }

    instancesetaddresses(instAddress);
}

uint32 inittestapplication(uint8 s1switch);


//returns the use case / operational mode
int decarangingmode(uint8 s1switch)
{
    int mode = 0;

    if(s1switch & SWS1_SHF_MODE)
    {
        mode = 1;
    }

    if(s1switch & SWS1_CH5_MODE)
    {
        mode = mode + 2;
    }

    return mode;
}

uint32 inittestapplication(uint8 s1switch)
{
	uint32 devID ;
	instanceConfig_t instConfig;
	int result;

	SPI_ConfigFastRate(DW1000_SPI_LOW);  //max SPI before PLLs configured is ~4M

	//this is called here to wake up the device (i.e. if it was in sleep mode before the restart)
	devID = instancereaddeviceid();

	if(DWT_DEVICE_ID != devID) { //if the read of device ID fails, the DW1000 could be asleep
		port_SPIx_clear_chip_select();  //CS low
		//200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
		port_Delay_MS(1);
		port_SPIx_set_chip_select();  //CS high
		//200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
		port_Delay_MS(7);
		devID = instancereaddeviceid();
	}
	printf("DW1000 Device Id:%x\n",(unsigned int)devID);
	// SPI not working or Unsupported Device ID
	if(DWT_DEVICE_ID != devID)
		return((uint32_t)-1) ;

	result = instance_init();
	if (0 > result) return((uint32_t)-1) ; // Some failure has occurred

	SPI_ConfigFastRate(DW1000_SPI_HIGH); //increase SPI to max
	devID = instancereaddeviceid() ;

	if (DWT_DEVICE_ID != devID) { // Means it is NOT DW1000 device
		// SPI not working or Unsupported Device ID
		return((uint32_t)-1) ;
	}

	if((s1switch & SWS1_ANC_MODE) == 0) {
		instance_mode = TAG;
	} else {
		instance_mode = ANCHOR;
	}

	addressconfigure(s1switch, instance_mode) ;                            // set up initial payload configuration

	if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)) {
		instance_mode = LISTENER;
	}

	instancesetrole(instance_mode) ;     // Set this instance role

	// get mode selection (index) this has 4 values see chConfig struct initialiser for details.
	dr_mode = decarangingmode(s1switch);

	chan = instConfig.channelNumber = chConfig[dr_mode].channel ;
	instConfig.preambleCode = chConfig[dr_mode].preambleCode ;
	instConfig.pulseRepFreq = chConfig[dr_mode].prf ;
	instConfig.pacSize = chConfig[dr_mode].pacSize ;
	instConfig.nsSFD = chConfig[dr_mode].nsSFD ;
	instConfig.sfdTO = chConfig[dr_mode].sfdTO ;
	instConfig.dataRate = chConfig[dr_mode].datarate ;
	instConfig.preambleLen = chConfig[dr_mode].preambleLength ;

	instance_config(&instConfig, &sfConfig[dr_mode]) ;                  // Set operating channel etc

	return devID;
}
/**
**===========================================================================
**
**  Abstract: main program
**
**===========================================================================
*/
void process_dwRSTn_irq(void)
{
	instance_notify_DW1000_inIDLE(1);
}

void process_deca_irq(void)
{
	do {
		instance_process_irq(0);
	} while(port_CheckEXT_IRQ() == 1); //while IRS line active (ARM can only do edge sensitive interrupts)
}

void configure_continuous_txspectrum_mode(uint8 s1switch)
{
    //configure DW1000 into Continuous TX mode
    instance_starttxtest(0x1000);
    //measure the power
    //Spectrum Analyser set:
    //FREQ to be channel default e.g. 3.9936 GHz for channel 2
    //SPAN to 1GHz
    //SWEEP TIME 1s
    //RBW and VBW 1MHz
    //measure channel power

    //user has to reset the board to exit mode
    while(1)
    {
			port_Delay_MS(2);
//        xtimer_usleep(2000);
    }

}

extern SemaphoreHandle_t xSemaphoreUpdate;
/*
 * @fn      main()
 * @brief   main entry point
**/
int rx = 0;
int instance_main(void)
{
    int Unit = 1, Unitid = 2, Mode = 0;

		printf("    DW1000 Device Configuration.    \n");
		printf("Mode Config:  Mode-%d.\n", Mode);
		printf("       UNIT:  %s.\n", (Unit)? "ANCHOR":"TAG");
		printf("    UNIT ID:  %d.\n", Unitid);

		if(Unitid == 1) Unitid = 4;

		s1switch = 0x01 | ( Unit << 3 ) | ( Mode << 1) | ( Unitid  << 4 );

    port_DisableEXT_IRQ(); //disable ScenSor IRQ until we configure the device

    //run DecaRangeRTLS application for TREK
		if(inittestapplication(s1switch) == (uint32)-1) {
				printf(" inittestapllication  failed\n");
				return 0; //error
		}

		// Is continuous spectrum test mode selected?
		if((s1switch & SWS1_TXSPECT_MODE) == SWS1_TXSPECT_MODE) {
				//this function does not return!
				configure_continuous_txspectrum_mode(s1switch);
		}

    port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting

    // main loop
    while(1)
    {
        int monitor_local = instance_data[0].monitor ;
        int txdiff = (portGetTickCnt() - instance_data[0].timeofTx);

        instance_run();
        instance_mode = instancegetrole();

        //if delayed TX scheduled but did not happen after expected time then it has failed... (has to be < slot period)
        //if anchor just go into RX and wait for next message from tags/anchors
        //if tag handle as a timeout
        if((monitor_local == 1) && ( txdiff > instance_data[0].slotPeriod))
        {
            uint32 tdly ;
            uint32 reg1, reg2;

            reg1 = dwt_read32bitoffsetreg(0x0f, 0x1);
            reg2 = dwt_read32bitoffsetreg(0x019, 0x1);
            tdly = dwt_read32bitoffsetreg(0x0a, 0x1);

            printf("T%08x %08x time %08x %08x \n", (unsigned int) reg2, (unsigned int) reg1,
                    (unsigned int) dwt_read32bitoffsetreg(0x06, 0x1), (unsigned int) tdly); /* TBD */

            instance_data[0].wait4ack = 0;

            if(instance_mode == TAG)
            {
                inst_processrxtimeout(&instance_data[0]);
            }
            else //if(instance_mode == ANCHOR)
            {
                dwt_forcetrxoff();  //this will clear all events
                //dwt_rxreset();
                //enable the RX
                instance_data[0].testAppState = TA_RXE_WAIT ;
            }
            instance_data[0].monitor = 0;
        }

        rx = instancenewrange();

        //if there is a new ranging report received or a new range has been calculated, then prepare data
        //to output over USB - Virtual COM port, and update the LCD
        if(rx != TOF_REPORT_NUL)
        {
//            int l = 0, r= 0, aaddr, taddr;
//            int rangeTime, valid;
            //int correction ;
//            uint16 txa, rxa;

            //send the new range information to LCD and/or USB
//            aaddr = instancenewrangeancadd() & 0xf;
//            taddr = instancenewrangetagadd() & 0xf;
//            rangeTime = instancenewrangetim() & 0xffffffff;

//            l = instancegetlcount() & 0xFFFF;
//            if(instance_mode == TAG)
//            {
//                r = instancegetrnum();
//            }
//            else
//            {
//                r = instancegetrnuma(taddr);
//            }
//            txa =  instancetxantdly();
//            rxa =  instancerxantdly();
//            valid = instancevalidranges();

            if(rx == TOF_REPORT_T2A)
            {
                //correction = instance_data[0].tagSleepCorrection2;
                // anchorID tagID range rangeraw countofranges rangenum rangetime txantdly rxantdly address

//                printf("mc %02x %08x %08x %08x %08x %04x %02x %08x %c%d:%d \n",
//                       valid, instancegetidist_mm(0), instancegetidist_mm(1),
//                       instancegetidist_mm(2), instancegetidist_mm(3), l, r,
//                       rangeTime, (instance_mode == TAG)?'t':'a', taddr, aaddr);
xSemaphoreGive(xSemaphoreUpdate);
//								/* TWR RAW Report */
//								printf("mr %02x %08x %08x %08x %08x %04x %02x %04x%04x %c%d:%d\r\n",
//												valid, instancegetidistraw_mm(0), instancegetidistraw_mm(1),
//												instancegetidistraw_mm(2), instancegetidistraw_mm(3),
//												l, r, txa, rxa, (instance_mode == TAG)?'t':'a', taddr, aaddr);
            } else { //anchor to anchor ranging
//                printf( "ma %02x %08x %08x %08x %08x %04x %02x %08x a0:%d\r\n",
//                        valid, instancegetidist_mm(0), instancegetidist_mm(1),
//                        instancegetidist_mm(2), instancegetidist_mm(3),
//                        l, instancegetrnumanc(0), rangeTime, aaddr);
            }
//            instancecleardisttableall();
        } //if new range present
    }


//    return 0;
}
